/*
 * ChangeLog
 * Date             Author          Notes
 * 2015.12.08       Herbert         修复了选针器后４复位崩溃的bug
 */
#include "selector.h"
#include "dispatch.h"
#include "act_types.h"
#include "string.h"

/*
 * @param index: 组内序号0-7
 * @param pstate: 所有刀片更新后的状态
 */
int selector_exec(MH_UDP *pudp, int index, uchar *pstate)
{
    CAN_MSG msg = { 0, 0, { 0 } };

    int state = 0;
    int c = 0;
    msg.id = ID_SEL;
    msg.len = 8;

    for (c = 0; c < 2; c++)
    {
        int d1 = 8 * c;
        int d2 = 4 * c;
        state = ((pstate[0 + d1] & (0x01 << index)) != 0);
        msg.data[0 + d2] = (state << 7) | (index << 4);
        state = ((pstate[2 + d1] & (0x01 << index)) != 0);
        msg.data[0 + d2] |= (state << 3) | index;

        state = ((pstate[1 + d1] & (0x01 << index)) != 0);
        msg.data[1 + d2] = (state << 7) | (index << 4);
        state = ((pstate[3 + d1] & (0x01 << index)) != 0);
        msg.data[1 + d2] |= (state << 3) | index;

        state = ((pstate[4 + d1] & (0x01 << index)) != 0);
        msg.data[2 + d2] = (state << 7) | (index << 4);
        state = ((pstate[6 + d1] & (0x01 << index)) != 0);
        msg.data[2 + d2] |= (state << 3) | index;

        state = ((pstate[5 + d1] & (0x01 << index)) != 0);
        msg.data[3 + d2] = (state << 7) | (index << 4);
        state = ((pstate[7 + d1] & (0x01 << index)) != 0);
        msg.data[3 + d2] |= (state << 3) | index;

    }

    return dispatch_msg(pudp, &msg, NULL);
}

int selector_group_exec(MH_UDP *pudp, int index, uchar *pstate)
{
      CAN_MSG msg = { 0, 0, { 0 } };

    int state = 0;
    int c = 0;
    msg.id = ID_SEL;
    msg.len = 8;

    for (c = 0; c < 2; c++)
    {
        int d1 = 8 * c;
        int d2 = 4 * c;
        state = ((pstate[0 + d1] & (0x01 << index)) != 0);
        msg.data[0 + d2] = (state << 7) | (index << 4);
        state = ((pstate[2 + d1] & (0x01 << index)) != 0);
        msg.data[0 + d2] |= (state << 3) | index;

        state = ((pstate[1 + d1] & (0x01 << index)) != 0);
        msg.data[1 + d2] = (state << 7) | (index << 4);
        state = ((pstate[3 + d1] & (0x01 << index)) != 0);
        msg.data[1 + d2] |= (state << 3) | index;

        state = ((pstate[4 + d1] & (0x01 << index)) != 0);
        msg.data[2 + d2] = (state << 7) | (index << 4);
        state = ((pstate[6 + d1] & (0x01 << index)) != 0);
        msg.data[2 + d2] |= (state << 3) | index;

        state = ((pstate[5 + d1] & (0x01 << index)) != 0);
        msg.data[3 + d2] = (state << 7) | (index << 4);
        state = ((pstate[7 + d1] & (0x01 << index)) != 0);
        msg.data[3 + d2] |= (state << 3) | index;
    }

    return dispatch_msg(pudp, &msg, NULL);
}

/*
 * @param group: 选针器组序号
 * @param index: 组内序号0-7
 */
int selector_reset(MH_UDP *pudp, int group, int index)
{
    CAN_MSG msg = { 0, 0, { 0 } };

    int g = (group > 7) ? group - 8 : group;
    msg.id = (group > 7) ? ID_SEL_RESET_R : ID_SEL_RESET;
    msg.len = 8;
    msg.data[g] = 0x01 << index;

    return dispatch_msg(pudp, &msg, NULL);
}

/*
 * @param index: 组内序号0-7
 * @param type: 0-左机头；1-右机头
 * @note: 复位所有组的第index个刀片
 */
int selector_reset_all(MH_UDP *pudp, int index, int type)
{
    CAN_MSG msg = { 0, 0, { 0 } };

    msg.id = (type == LEFT_CARRIAGE) ? ID_SEL_RESET : ID_SEL_RESET_R;
    msg.len = 8;
    msg.data[0] = 0x01 << index;
    msg.data[1] = 0x01 << index;
    msg.data[2] = 0x01 << index;
    msg.data[3] = 0x01 << index;
    msg.data[4] = 0x01 << index;
    msg.data[5] = 0x01 << index;
    msg.data[6] = 0x01 << index;
    msg.data[7] = 0x01 << index;

    return dispatch_msg(pudp, &msg, NULL);
}
